﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Maubot.Modeling;

namespace Maubot.MathFunctions
{
    public class Calcolator
    {
        public static double DeltaX(Position current, Position target)
        {
            return target.X - current.X;
        }
        public static double DeltaY(Position current, Position target)
        {
            return target.Y - current.Y;
        }

        public static double PointsDistance(Position current, Position target)
        {
            return Math.Sqrt(Math.Pow(DeltaX(current, target), 2) + Math.Pow(DeltaY(current, target), 2));
        }

        public static double PointsAngle(Position current, Position target)
        {
            double result = 90 - RadianToDegree(Math.Atan2(DeltaY(current, target), DeltaX(current, target)));

            return NormalizeAngle(result, 180);
        }

        public static double DeltaAngle(Pose current, Position target)
        {
            double angle = PointsAngle(current, target);
            double result = angle - current.Angle;

            return NormalizeAngle(result, 180);
        }

        public static double DeltaInvariantAngle(Pose current, Position target)
        {
            double angle = PointsAngle(current, target);
            double result = angle - current.Angle;
            
            return NormalizeAngle(result, 90);
        }

        public static double RadianToDegree(double angle)
        {
            return angle * 180.0 / Math.PI;
        }

        public static double DegreeToRadiant(double angle)
        {
            return angle / 180.0 * Math.PI;
        }

        public static double NormalizeAngle(double result, int halfAmplitude)
        {
            while (result > halfAmplitude)
                result -= halfAmplitude * 2;

            while (result < - halfAmplitude)
                result += halfAmplitude * 2;

            return result;
        }
    }
}
